#include <stdlib.h>
#include "global.h"
#include "gcode.h"
#include "delay.h"
#include "keyboard.h"
#include "manualmode.h"
#include "ili9320.h"

const char axisName[5] = "XYZE";

int SetStepsperKeypress(void)
{
    int k = 100;
    uint8_t c = 0;
    LCD_Clear(Black);
    win_showMsgWin();
    scr_setScroll(false);
    scr_puts("\n ->Up      [U]");
    scr_puts("\n ->Down    [D]");
    scr_puts("\n ->Cancel  [L]");
    scr_puts("\n ->OK      [R]");
    scr_gotoxy(2, 6);
    scr_printf("step per key press: %f mm ", (float)k / 100);
    do
    {
        c = kbd_GetKey();
        if (c == KEY_U_DOWN) k += 1;
        if (c == KEY_D_DOWN)    k -= 1;
        scr_gotoxy(2, 6);
        scr_printf("step per key press: %f mm ", (float)k / 100);
    } while (c != KEY_L_DOWN && c != KEY_R_DOWN);
    if (c == KEY_L_DOWN)
    {
        return 0;
    }
    else
    {
        return k * SM_X_STEPS_PER_MM / 100;
    }
}



void manualMode(void)
{
    static uint8_t limits = 0xFF;
    const double axisK[4] = { SM_X_STEPS_PER_MM, SM_Y_STEPS_PER_MM, SM_Z_STEPS_PER_MM, SM_E_STEPS_PER_MM };
    int i, k = SM_X_STEPS_PER_MM;
    int c = -1, pos = 0, v = 0;

    k = SetStepsperKeypress();
    if (k == 0)return;

    uint32_t frq[4] =
    {
        SM_MANUAL_MODE_STEPS_PER_SEC * K_FRQ,
        SM_MANUAL_MODE_STEPS_PER_SEC * K_FRQ,
        SM_MANUAL_MODE_STEPS_PER_SEC * K_FRQ,
        SM_MANUAL_MODE_STEPS_PER_SEC * K_FRQ
    };
    const char xname[4][7] = { "X-axis", "Y-axis", "Z-axis", "Ex" };
    uint8_t dir[4] = { 0, 0, 0, 0 };
    uint32_t steps[4] = { 0, 0, 0, 0 };

    LCD_Clear(Black);
    scr_setfullTextWindow();
    scr_gotoxy(6, 0);
    scr_fontColor(Green, Black);
    scr_puts("CNC MANUAL MODE");
    scr_fontColor(Yellow, Black);
    //    scr_puts("\n  key '6' - [X+]  key '4' - [X-] ");
    //    scr_puts("\n  key '2' - [Y+]  key '8' - [Y-] ");
    //    scr_puts("\n  key '1' - [Z+]  key '7' - [Z-] ");
    //    scr_puts("\n  key '3' - [E+]  key '9' - [E-] ");
    //    scr_puts("\n  'A'k+1 'B'k-1 '*'k=1 '#'k++");
    //    scr_puts("\n step motors 'D' - full stop");
    //    scr_puts("\n 0 - zero  5 - goto zero C - exit");
    scr_puts("\n -Left  [L]  -Right [R]");
    scr_puts("\n -Up    [U]  -Down  [D]");
    scr_puts("\n -OK    [C]");
    delay_ms(700);
    //    while (kbd_getKey() >= 0) {};
    do
    {
        scr_fontColor(White, Black);
        for (i = 0; i < 3; i++)
        {
            double n = (double)stepm_getCurGlobalStepsNum(i) / axisK[i];
            scr_gotoxy(1 + i * 10, TEXT_Y_MAX - 4);
            scr_printf("%c:%f  ", axisName[i], n);
            if (i < 3)
            {
                scr_gotoxy(1, TEXT_Y_MAX - 3 + i);
                scr_printf("steps %c:%d      ", axisName[i], stepm_getCurGlobalStepsNum(i));
            }
            dir[i] = 0;
            steps[i] = 0;
        }
        scr_fontColor(Blue, Black);
        scr_gotoxy(2, TEXT_Y_MAX - 7);
        scr_printf("step per key press: %f mm ", (double)k / SM_X_STEPS_PER_MM);


        scr_gotoxy(5, 5);
        scr_fontColorNormal();
        for (uint8_t j = 0; j < 4; j++)
        {
            scr_printf("%s ", xname[j]);
        }
        scr_fontColorInvers();
        scr_gotoxy(pos * 7 + 5, 5);
        scr_printf("%s", xname[pos]);


        c = kbd_GetKey();
        if (c == KEY_L_DOWN)    pos = pos <= 0 ? 3 : pos - 1;
        if (c == KEY_R_DOWN)    pos = pos >= 3 ? 0 : pos + 1;
        if (c == KEY_U_DOWN)    v = 1;
        if (c == KEY_D_DOWN)    v = 0;

        steps[pos] = k;
        dir[pos] = v;
        stepm_addMove(steps, frq, dir);

        if (limits != limits_chk())
        {
            limits = limits_chk();
            scr_Rectangle(304, 232, 309, 239, limitX_chk() ? Red : Green, true);
            scr_Rectangle(310, 232, 315, 239, limitY_chk() ? Red : Green, true);
            scr_Rectangle(314, 232, 319, 239, limitZ_chk() ? Red : Green, true);
        }
    } while (c != KEY_C_DOWN);
}
